#include "stm32f10x.h"                  // Device header
#include "ServoControl.h"
#include "Delay.h"
#include "StepData.h"

void Move_Forward( void)
{
	for( int i = 0; i < 30; i++)
	{
		
		SC_SetLegR2( forward[i][0], forward[i][1],forward[i][2]);
		SC_SetLegR1( forward[i][3], forward[i][4],forward[i][5]);		
		SC_SetLegL1( -forward[i][6], forward[i][7],forward[i][8]);
		SC_SetLegL2( -forward[i][9], forward[i][10], forward[i][11]);
		SC_SetLegL3( -forward[i][12], forward[i][13], forward[i][14]);
		SC_SetLegR3( forward[i][15], forward[i][16], forward[i][17]);
		
	}
}

void Move_Backward( void)
{
	for( int i = 0; i < 30; i++)
	{
		
		SC_SetLegR2( -forward[i][0], forward[i][1],forward[i][2]);
		SC_SetLegR1( -forward[i][3], forward[i][4],forward[i][5]);		
		SC_SetLegL1( forward[i][6], forward[i][7],forward[i][8]);
		SC_SetLegL2( forward[i][9], forward[i][10], forward[i][11]);
		SC_SetLegL3( forward[i][12], forward[i][13], forward[i][14]);
		SC_SetLegR3( -forward[i][15], forward[i][16], forward[i][17]);
		
	}
}

void Move_Stop( void)
{
	
		
		SC_SetLegR2(  Stop[0], Stop[1],  Stop[2]);
		SC_SetLegR1(  Stop[3], Stop[4],  Stop[5]);		
		SC_SetLegL1( -Stop[6], Stop[7],  Stop[8]);
		SC_SetLegL2( -Stop[9], Stop[10], Stop[11]);
		SC_SetLegL3( -Stop[12],Stop[13], Stop[14]);
		SC_SetLegR3(  Stop[15],Stop[16], Stop[17]);
		
	
}

void Move_Rotate_Left( void)
{
	for( int i = 0; i < 16; i++)
	{
		SC_SetLegL1( -Rotate_Left[i][0],  Rotate_Left[i][1],  Rotate_Left[i][2]);
		SC_SetLegL2( -Rotate_Left[i][3],  Rotate_Left[i][4],  Rotate_Left[i][5]);
		SC_SetLegL3( -Rotate_Left[i][6],  Rotate_Left[i][7],  Rotate_Left[i][8]);
		SC_SetLegR1(  Rotate_Left[i][9],  Rotate_Left[i][10], Rotate_Left[i][11]);		
		SC_SetLegR2(  Rotate_Left[i][12], Rotate_Left[i][13], Rotate_Left[i][14]);
		SC_SetLegR3(  Rotate_Left[i][15], Rotate_Left[i][16], Rotate_Left[i][17]);
		
		
		
	}
}
void Move_Rotate_Right( void)
{
	for( int i = 0; i < 16; i++)
	{
		SC_SetLegL1(  Rotate_Left[i][0],  Rotate_Left[i][1],  Rotate_Left[i][2]);
		SC_SetLegL2(  Rotate_Left[i][3],  Rotate_Left[i][4],  Rotate_Left[i][5]);
		SC_SetLegL3(  Rotate_Left[i][6],  Rotate_Left[i][7],  Rotate_Left[i][8]);
		SC_SetLegR1( -Rotate_Left[i][9],  Rotate_Left[i][10], Rotate_Left[i][11]);		
		SC_SetLegR2( -Rotate_Left[i][12], Rotate_Left[i][13], Rotate_Left[i][14]);
		SC_SetLegR3( -Rotate_Left[i][15], Rotate_Left[i][16], Rotate_Left[i][17]);	
	}
}
